
/*
 * Include Files
 *
 */
#if defined(MATLAB_MEX_FILE)
#include "tmwtypes.h"
#include "simstruc_types.h"
#else
#define SIMPLIFIED_RTWTYPES_COMPATIBILITY
#include "rtwtypes.h"
#undef SIMPLIFIED_RTWTYPES_COMPATIBILITY
#endif



/* %%%-SFUNWIZ_wrapper_includes_Changes_BEGIN --- EDIT HERE TO _END */
#include <math.h>

#define    kp_spd   0.005
#define    ki_spd   0.08
#define    max      5.5
#define    min      -5.5
/* %%%-SFUNWIZ_wrapper_includes_Changes_END --- EDIT HERE TO _BEGIN */
#define u_width 1
#define y_width 1

/*
 * Create external references here.  
 *
 */
/* %%%-SFUNWIZ_wrapper_externs_Changes_BEGIN --- EDIT HERE TO _END */
 
/* %%%-SFUNWIZ_wrapper_externs_Changes_END --- EDIT HERE TO _BEGIN */

/*
 * Output function
 *
 */
void spdLoop_Outputs_wrapper(const real32_T *spd_err,
			real32_T *iq_cmd)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
static int spd_cnt;
    spd_cnt++;
    static float integrator;
    static float zone;
    if(spd_cnt >= 10)
    {
        spd_cnt = 0;
        float out = kp_spd * *spd_err + 0.0001f * integrator;

        if(out > max)
        {
            out = max;
            zone -= max;
        }
        else if(out < min)
        {
            out = min;
            zone -= min;
        }
        else
        {
            zone = 0;
        }
        float integratorGain = ki_spd * *spd_err;
        int tmp0 = zone > 0.0f ? 1 : -1;
        int tmp1 = integratorGain > 0.0f? 1 : -1;
        if((zone != 0.0f) && (tmp0 == tmp1))
        {
            integratorGain = 0;
        }
        integrator += integratorGain;
        *iq_cmd = out;
    }
    else
    {
        *iq_cmd = *iq_cmd;
    }
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
}


